/*
 *  WMovingObject.cpp
 *  GLRender
 *
 *  Created by Patrick zulian on 5/4/10.
 *  Copyright 2010 University  of Lugano. All rights reserved.
 *
 */

#include "WMovingObject.h"
#include "Physics.h"


WMovingObject::WMovingObject(const Point3d &position, const Point3d &velocity0, const double _mass, const double radius )
: WObject(position, _mass, radius), _velocity(velocity0), _nextVelocity(velocity0), acceleration(0), accelerationFactor(1.0), collided(false)
{
	alpha = beta = gamma = 0.0;
	rotAngle=0;
	vertAngle=0;
	double speed = _velocity.norm();
	_upperNormal = (_velocity/speed )^Point3d(1, 0, 0);
}

void 
WMovingObject::step(const double tau)
{
	if (collided) {
		_velocity = _nextVelocity;
		collided = false;
	}
	
	Physics::getInstance()->rotateObject( this, alpha, beta, gamma );
	alpha = beta = gamma = 0.0;
	
	_position += tau*_velocity;
	

	if (acceleration == 0.0) {
		return;
	}
	
	
	double norm = _velocity.norm();
	
		
	if (norm < getMinSpeed()) {
		_velocity.normalize();
		_velocity *= getMinSpeed();
		acceleration = 0;
		return;
	}
	else if (norm > getMaxSpeed()) {
		_velocity.normalize();
		_velocity *= getMaxSpeed();
		acceleration = 0;
		return;
	}
	
	_velocity *= (1.0 +  tau*acceleration/norm);	
	acceleration = 0;

}

void 
WMovingObject::pitch(const double angle)
{	
	alpha += angle;
	vertAngle-= 180.0 * angle/3.1416;
}

void 
WMovingObject::roll(const double angle)
{	
	beta += angle;
	rotAngle -= 180.0 * angle/3.1416;
}

void 
WMovingObject::yaw(const double angle)
{
	gamma += angle;
}


void 
WMovingObject::speedUp()
{
	acceleration = accelerationFactor;
}


void 
WMovingObject::speedDown()
{
	acceleration = -accelerationFactor;
}

